//
// Created by 孟令涛 on 24-7-15.
//
#include "CarMovementTask.h"
#include "tim.h"
#include "GlobalVars.h"

#define PWM_FREQUENCY 1000 // 1kHz
#define PWM_DUTY_CYCLE 50  // 50%
#define FULL_SPEED 500
#define HALF_SPEED 250


void CarMovementTask(void *argument) {
    for (;;) {
        if (currentMode == MODE_LINE_FOLLOWING) {
            if (fire_state) {
                switch (fire_movement_state) {
                    case 1:
                        Motor_Forward(150, 150);
                        break;
                    case 2:
                        Motor_Backward(150, 150);
                        break;
                    case 3:
                        Turn_Left(150);
                        break;
                    case 4:
                        Turn_Right(150);
                        break;
                    case 5:
                        Spin_Left(150);
                        break;
                    case 6:
                        Spin_Right(150);
                        break;
                    default:
                        Motor_Stop(150, 150);
                        break;
                }
            } else {
                switch (control_movement_state) {
                    case 1:
                        Motor_Forward(carControl_left_speed, carControl_right_speed);
                        break;
//            case 2:
//                Motor_Backward(FULL_SPEED, FULL_SPEED);
//                break;
                    case 3:
                        Turn_Left(carControl_right_speed);
                        break;
                    case 4:
                        Turn_Right(carControl_left_speed);
                        break;
                    case 5:
                        Spin_Left(carControl_left_speed);
                        break;
                    case 6:
                        Spin_Right(carControl_right_speed);
                        break;
                    default:
//            Motor_Forward(0, 0);
                        Motor_Stop(FULL_SPEED, FULL_SPEED);
                        break;
                }
            }
        } else {
            switch (movement_state) {
                case 1:
                    Motor_Forward(FULL_SPEED, FULL_SPEED);
                    break;
                case 2:
                    Motor_Backward(FULL_SPEED, FULL_SPEED);
                    break;
                case 3:
                    Turn_Left(HALF_SPEED);
                    break;
                case 4:
                    Turn_Right(HALF_SPEED);
                    break;
                case 5:
                    Spin_Left(HALF_SPEED);
                    osDelay(70);
                    break;
                case 6:
                    Spin_Right(HALF_SPEED);
                    osDelay(70);
                    break;
                default:
//            Motor_Forward(0, 0);
                    Motor_Stop(FULL_SPEED, FULL_SPEED);
                    break;
            }
            //        movement_state = (movement_state + 1) % 6;

        }
        osDelay(10); // 每个动作持续2�???????

    }
}

void Motor_Forward(uint16_t left_speed, uint16_t right_speed) {
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_SET);   // 左电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_RESET); // 左电机后
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_SET);   // 右电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_RESET); // 右电机后

    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, left_speed);  // 设置左电机PWM
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, right_speed); // 设置右电机PWM
    Motor_Forward_Flag = 0;
}

void Motor_Backward(uint16_t left_speed, uint16_t right_speed) {
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_RESET); // 左电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_SET);   // 左电机后
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_RESET); // 右电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_SET);   // 右电机后

    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, left_speed);  // 设置左电机PWM
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, right_speed); // 设置右电机PWM
}

void Turn_Left(uint16_t speed) {
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_SET);   // 左电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_RESET); // 左电机后
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_SET);   // 右电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_RESET); // 右电机后

    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, 0);     // 左电机停�???????
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, speed); // 右电机前�???????
}

void Turn_Right(uint16_t speed) {
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_SET);   // 左电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_RESET); // 左电机后
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_SET);   // 右电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_RESET); // 右电机后

    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, speed); // 左电机前�???????
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, 0);     // 右电机停�???????
}

void Spin_Left(uint16_t speed) {
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_RESET); // 左电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_SET);   // 左电机后
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_SET);   // 右电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_RESET); // 右电机后

    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, speed); // 左电机后�???????
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, speed); // 右电机前�???????
}

void Spin_Right(uint16_t speed) {
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_SET);   // 左电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_RESET); // 左电机后
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_RESET); // 右电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_SET);   // 右电机后

    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, speed); // 左电机前�???????
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, speed); // 右电机后�???????
}

void Motor_Stop(uint16_t left_speed, uint16_t right_speed) {
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_SET);   // 左电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_SET); // 左电机后
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_SET);   // 右电机前
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_SET); // 右电机后

    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, left_speed);  // 设置左电机PWM
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, right_speed); // 设置右电机PWM
}
